#include <rtthread.h>
#include <stdio.h>

#include <mopenocd.h>
#include <adapter/rttBbGPIO.h>

using namespace openocd;

#define OCD_ADNAME_DEFAULT "tdi:pb9,tms:pa8,tck:pc6,tdo:pd14,srs:pd12"

static bool _target_init(adapter &ad, targetIO &tio, jtag_tap &tap, const char *adev = 0)
{
    if (!adev)
        adev = OCD_ADNAME_DEFAULT;

    if (!ad.open(adev))
    {
        printf("open adapter fail\n");
        return false;
    }

    tap.bind(&ad);
    if (tap.init() != 0)
    {
        printf("jtag init fail\n");
        return false;
    }

    tap.jtag_init_inner();

    tio.bind(&tap);
    if (tio.init() != 0)
    {
        printf("target init fail\n");
        return false;
    }

    if (tio.examine() != 0)
    {
        printf("examine fail\n");
        return false;
    }

    return true;
}

static int ocdtest(int c, char **v)
{
    rttBbGPIO adapter;
    jtag_tap jtag;
    targetRISCV tio;
    int ret;

    if (!adapter.open("trs:pb9,tdi:pa8,tms:pc6,tck:pd14,tdo:pd12"))
        goto _out;

    jtag.bind(&adapter);
    jtag.init();
    jtag.jtag_init_inner();

    tio.bind(&jtag);
    tio.init();
    ret = tio.examine();
    if (ret != 0)
        goto _out;

    tio.halt();
    tio.pollCurStatus();//must be called after halt()

    rt_thread_mdelay(5000);
    tio.resume(0, 0, 0, 0);

_out:
    printf("examine out\n");

    return 0;
}
MSH_CMD_EXPORT(ocdtest, ocdtest);

static int ocdmwr(int ac, char **av)
{
    rttBbGPIO ad;
    jtag_tap jtag;
    targetRISCV tio;
    target_addr_t a = 0x80067e40;
    uint32_t v = 0x12345678, r = 0;

    if (!_target_init(ad, tio, jtag))
    {
        printf("init fail\n");
        return -1;
    }

    tio.halt();
    tio.pollCurStatus();//must be called after halt()
    tio.writeMem32(a, v);
    tio.readMem32(a, r);
    tio.resume(0, 0, 0, 0);

    printf("read: 0x%x\n", r);

    return 0;
}
MSH_CMD_EXPORT(ocdmwr, memory write and read);
